//---------------------------------------------------------------------------


#pragma hdrstop
#include "stdafx.h"
#include "MainProcess.h"

//---------------------------------------------------------------------------

#pragma package(smart_init)

bool bExecuteInitial = false;
TimeDifferenceStrcut Time_SensorAccess_Interval = {0,0,0};
TimeDifferenceStrcut Time_SensorAccess_Execution = {0,0,0};
TimeDifferenceStrcut Time_RenewMotion_Interval = {0,0,0};
TimeDifferenceStrcut Time_RenewMotion_Execution = {0,0,0};
TimeDifferenceStrcut Time_MainProcess_Interval = {0,0,0};
TimeDifferenceStrcut Time_MainProcess_Execution = {0,0,0};

void Initialize(void)
{
	MotorSchedule_Init();
	HeadSchedule_Init();
  Motion_Init();
	Head_Init();
	HMI_Init();
	Comp_Init();
	Board_Init();

	MotorWritten = false;
	HeadWritten = false;

	bExecuteInitial = false;
}

void SensorAccess(void)
{
	static unsigned char id_index_main = 0;
	static unsigned char id_index_other = READ_ID_INDEX_OTHER_LOWERBOUND;
	//static bool read_main = true;
	unsigned char id_index_other_lowerbound = 0;

	Time_SensorAccess_Interval.Now = GetCPUTime();
	Time_SensorAccess_Execution.Past = Time_SensorAccess_Interval.Now;
	CalculateTimeDifference_ms(&Time_SensorAccess_Interval);
	Time_SensorAccess_Interval.Past = Time_SensorAccess_Interval.Now;
	#ifdef bShow_SensorAccessInterval
		if(bShow_SensorAccessInterval)
		{
			Memo_ShowInfo->Lines->Add("SensorAccess Interval: " +
																FormatFloat("0.00", Time_SensorAccess_Interval.dT) + " ms");
		}
	#endif


	// Read motor
	ReadMotor(id_index_main);
	id_index_main++;

	if(id_index_main >= id_index_other_lowerbound)
	{
		id_index_main = 0;
	}

	ReadMotor(id_index_other);
	id_index_other++;

	if(id_index_other >= MOTOR_NUM)
	{
		id_index_other = id_index_other_lowerbound;
	}

	// Access board
	BoardRX2Receive();

	// Send sensor information to Comp
	if(bCommunicateCompWithShareMemory)
	{
    Comp_StoreToShareM();
	}
	else
	{
		CompSend();
	}

	if(Board_Receive[BOARD_R_DIP] < 100)
	{
		if(bCommunicateHMIWithShareMemory)
		{
			//Memo_ShowInfo->Lines->Add("HMI");
			HMI_LoadFromShareM();
		}
		else
		{
			HMIRX2Receive();
		}
	}

	Time_SensorAccess_Execution.Now = GetCPUTime();
	CalculateTimeDifference_ms(&Time_SensorAccess_Execution);
	#ifdef bShow_SensorAccessExecutionTime
		if(bShow_SensorAccessExecutionTime)
		{
			Memo_ShowInfo->Lines->Add("SensorAccess Execution: " +
																FormatFloat("0.00", Time_SensorAccess_Execution.dT) + " ms");
		}
	#endif
}

void RenewMotion(void)
{
	Time_RenewMotion_Interval.Now = GetCPUTime();
	Time_RenewMotion_Execution.Past = Time_RenewMotion_Interval.Now;
	CalculateTimeDifference_ms(&Time_RenewMotion_Interval);
	Time_RenewMotion_Interval.Past = Time_RenewMotion_Interval.Now;
	#ifdef bShow_RenewMotionInterval
		if(bShow_RenewMotionInterval)
		{
			Memo_ShowInfo->Lines->Add("RenewMotion Interval: " +
																FormatFloat("0.00", Time_RenewMotion_Interval.dT) + " ms");
		}
	#endif

	if(bExecuteInitial)
	{
		Initialize();
		Mtn_POSE_Standing(128, 0, 17);
	}

	if(Head.Mode_RX > 0)
	{
		Head.Mode = Head.Mode_RX;
		Head.Mode_RX = 0;
	}

	HeadMotion();
	if(HeadSchedule.NowIndex != HeadSchedule.EndIndex)
	{
		Update_HeadSchedule_to_MotorData();
	}

	if(MotorSchedule.Enabled == true) // Add in 2014/07/28
	{
		Update_MotorSchedule_to_MotorData();
  }
	if((HeadWritten == true) && (MotorWritten == false))
	{
		WriteMotor(UPPER_BODY);
		HeadWritten = false;
	}

	if(bCommunicateCompWithShareMemory)
	{
		Comp_StoreToShareM();
	}
	else
	{
		CompSend();
	}

	if(Board_Receive[BOARD_R_DIP] >= 100)
	{
		if(bCommunicateCompWithShareMemory)
		{
      Comp_LoadFromShareM();
		}
		else
		{
			CompRX2Receive();
		}
	}
	else
	{
		if(bCommunicateHMIWithShareMemory)
		{
			//Memo_ShowInfo->Lines->Add("HMI");
      HMI_LoadFromShareM();
		}
		else
		{
      HMIRX2Receive();
    }
	}

	if((MotorSchedule.NowIndex == MotorSchedule.EndIndex) && (MotorSchedule.Delay_ExecutedCount == 0))
	{
    if(Motion.Mode_RX >= 100)
		{
			Motion.Mode = Motion.Mode_RX;
			Motion.ContinueAngle = Motion.ContinueAngle_RX;
			Motion.ContinueDistance = Motion.ContinueDistance_RX;
			Motion.Continue_Eanble = true;
			Motion.Continue_Set = Motion.Mode;
			Motion.Mode_RX = 0;
		}
		else if((Motion.Mode_RX > 0) && (Motion.Mode_RX < 100))
		{
			Motion.Mode = Motion.Mode_RX;
			Motion.ContinueAngle = Motion.ContinueAngle_RX;
			Motion.ContinueDistance = Motion.ContinueDistance_RX;
			Motion.Continue_Eanble = false;
			Motion.Continue_Set = OFF;
			Motion.Mode_RX = 0;
		}

		if((Motion.Continue_Eanble == true) || (Motion.Mode >= 100))
		{
			ContinuousMotion();
		}
		else
		{
			SingleMotion();
		}
	}
	else
	{
		CompRX.NowIndex = 0;
		CompRX.EndIndex = 0;
		COMP_PORT->InQueue->Clear();
	}

	Time_RenewMotion_Execution.Now = GetCPUTime();
	CalculateTimeDifference_ms(&Time_RenewMotion_Execution);
	#ifdef bShow_RenewMotionExecutionTime
		if(bShow_RenewMotionExecutionTime)
		{
			Memo_ShowInfo->Lines->Add("RenewMotion Execution: " +
																FormatFloat("0.00", Time_RenewMotion_Execution.dT) + " ms");
		}
	#endif
}

void SensorAccessManagement(void)
{
	// If the residual time (from now to the next time writing-motor) is enought for measuring sensor,
	// then execute SensorAccess();
	double time_residue = CalculateTimeDifference_ms(GetCPUTime(), Time_RenewMotion_Interval.Now);
	time_residue = abs(time_residue - TimerInterval_WriteMotor);

	if((time_residue > 10) && ((time_residue > Time_SensorAccess_Execution.dT + 1)) )
	{
		SensorAccess();
	}
}
